/*
   Memsic2125
   
   Read the Memsic 2125 two-axis accelerometer.  Converts the
   pulses output by the 2125 into milli-g's (1/1000 of earth's
   gravity) and prints them over the serial connection to the
   computer.
   
   The circuit:
	* X output of accelerometer to digital pin 2
	* Y output of accelerometer to digital pin 3
	* +V of accelerometer to +5V
	* GND of accelerometer to ground
  
   http://www.arduino.cc/en/Tutorial/Memsic2125
   
   created 6 Nov 2008
   by David A. Mellis
   modified 30 Jun 2009
   by Tom Igoe

 */
#define MAX_VALUE 600
#define DISTANCE_SCALE MAX_VALUE / NumLEDs

#define clockpin 13 // CI
#define enablepin 10 // EI
#define latchpin 9 // LI
#define datapin 11 // DI

#define xPin 6		// X output of the accelerometer
#define yPin 7		// Y output of the accelerometer

#define NumLEDs 1

int LEDChannels[NumLEDs][3] = {0};
int SB_CommandMode;
int SB_RedCommand;
int SB_GreenCommand;
int SB_BlueCommand;

int accelerationX;
int accelerationY;

int red;
int green;
int blue;

const int numReadings = 10;

int index = 0;                  // the index of the current reading
int xReadings[numReadings];      // the readings from the analog input
int yReadings[numReadings];      // the readings from the analog input
int xTotal = 0;                  // the running total
int yTotal = 0;                  // the running total
int xAverage = 0;                // the average
int yAverage = 0;                // the average



void setup() {
  for (int i = 0; i < numReadings; i++) {
    xReadings[i] = 0;          
    yReadings[i] = 0;          
  }
    
  // initialize serial communications:
  // initialize the pins connected to the accelerometer
  // as inputs:
  pinMode(xPin, INPUT);
  pinMode(yPin, INPUT);

   pinMode(datapin, OUTPUT);
   pinMode(latchpin, OUTPUT);
   pinMode(enablepin, OUTPUT);
   pinMode(clockpin, OUTPUT);
   SPCR = (1<<SPE)|(1<<MSTR)|(0<<SPR1)|(0<<SPR0);
   digitalWrite(latchpin, LOW);
   digitalWrite(enablepin, LOW);
}

void loop() {
  readAcceleration();
  translateAccelerationToColor();
  sendColorsToMegabrites();
  delay(2);
}

void averageAccel() {
  // subtract the last reading:
  xTotal = xTotal - xReadings[index];         
  yTotal = yTotal - yReadings[index];         
  // read from the sensor:  
  xReadings[index] = accelerationX;
  yReadings[index] = accelerationY; 
  // add the reading to the total:
  xTotal = xTotal + xReadings[index];       
  yTotal = yTotal + yReadings[index];       
  // advance to the next position in the array:  
  index = index + 1;                    

  // if we're at the end of the array...
  if (index >= numReadings)              
    // ...wrap around to the beginning: 
    index = 0;                           

  // calculate the average:
  xAverage = xTotal / numReadings;         
  yAverage = yTotal / numReadings;         
}

void sendColorsToMegabrites() {
  writeColorsToArray();
  WriteLEDArray();
}

void readAcceleration() {
  // variables to read the pulse widths:
  int pulseX, pulseY;
  // variables to contain the resulting accelerations
  
  // read pulse from x- and y-axes:
  pulseX = pulseIn(xPin,HIGH);
  pulseY = pulseIn(yPin,HIGH);
  
  // convert the pulse width into acceleration
  // accelerationX and accelerationY are in milli-g's: 
  // earth's gravity is 1000 milli-g's, or 1g.
  accelerationX = ((pulseX / 10) - 500) * 8;
  accelerationY = ((pulseY / 10) - 500) * 8;
  accelerationX = limit(accelerationX);
  accelerationY = limit(accelerationY);
  
  averageAccel();
}

void translateAccelerationToColor() {
  
  int value = (abs(xAverage) + abs(yAverage)) / 2;
  if (xAverage < 0) {
    blue = mapAccel(xAverage);
    if (accelerationY < 0) {
      green = mapAccel(yAverage);
    } else {
      red = mapAccel(yAverage);
    }
  } else {
    green = mapAccel(xAverage);
    if (accelerationY < 0) {
      red = mapAccel(yAverage);
    } else {
      blue = mapAccel(yAverage);
    }
  }
}

int mapAccel(int value) {
  return map(abs(value), 0, 1200, 0, 1023);
}

int limit(int value) {
  if (value > 1200) {
    return 1200;
  } else if (value < -1200) {
    return -1200;
  }
  
  return value;
}

void writeColorsToArray() {
    for (int i = 0; i < NumLEDs; i++) {
    LEDChannels[i][0] = getRed(i, red);
    LEDChannels[i][1] = getGreen(i, green);
    LEDChannels[i][2] = getBlue(i, blue);
  }
}

int getRed(int led, int intensity) {
  return getSimpleDistanceScaleValue(led, intensity);
}

int getBlue(int led, int intensity) {
  return getSimpleDistanceScaleValue(led, intensity);
}

int getGreen(int led, int intensity) {
  return getSimpleDistanceScaleValue(led, intensity);
}

int getSimpleDistanceScaleValue(int led, int intensity) {
  intensity -= led * DISTANCE_SCALE;
  return intensity;
}

void SB_SendPacket() {
 
    if (SB_CommandMode == B01) {
     SB_RedCommand = 120;
     SB_GreenCommand = 100;
     SB_BlueCommand = 100;
    }
 
    SPDR = SB_CommandMode << 6 | SB_BlueCommand>>4;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_BlueCommand<<4 | SB_RedCommand>>6;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_RedCommand << 2 | SB_GreenCommand>>8;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_GreenCommand;
    while(!(SPSR & (1<<SPIF)));
}
 
/* Return a visually-scaled value for the linear input value. Hand-tuned.
 */
int getScaledValue(int value) {
  if (value < 100) {
    if (value < 0) {
      value = 0;
    }
    return map(value, 0, 100, 0, 10);
  } else if (value < 200) {
    return map(value, 100, 200, 10, 100);
  } else if (value < 300) {
    return map(value, 200, 300, 100, 400);
  } else {
    if (value > MAX_VALUE) {
      value = MAX_VALUE;
    }
    return map(value, 300, MAX_VALUE, 400, 1023);
  }
}

void WriteLEDArray() {
    SB_CommandMode = B00; // Write to PWM control registers
    for (int h = 0;h<NumLEDs;h++) {
	  SB_RedCommand = normalize(getScaledValue(LEDChannels[h][0]));
	  SB_GreenCommand = normalize(getScaledValue(LEDChannels[h][1]));
	  SB_BlueCommand = normalize(getScaledValue(LEDChannels[h][2]));
	  SB_SendPacket();
    }
 
    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);
 
    SB_CommandMode = B01; // Write to current control registers
    for (int z = 0; z < NumLEDs; z++) SB_SendPacket();
    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);
}


int normalize(int blah) {
  if (blah < 0) {
    return 0;
  } else if (blah > 1023) {
    return 1023;
  }
  return blah;
}
